#!/usr/bin/env python3
#coding: utf-8

from ctypes import *
import time


class ControlLib:
	def __init__(self, connect_mode):
		self.connet_mode = connect_mode
		if self.connet_mode == "udp_mode":
			self.lib = cdll.LoadLibrary("/usr/lib/MiniBotControlLib.x64.so")

		elif self.connet_mode == "serial_mode":
			self.lib = cdll.LoadLibrary("/usr/lib/MiniBotControlLib.LinuxSerial.x64.so")
 
   #设置接收数据超时时间
	def SetRecvTimeOut(self,nNetTimeout):
		r = self.lib.SetRecvTimeOut(nNetTimeout)
		return r

   # 读取设备参数
	def GetParameters(self,id, address, size):
		cid = c_ubyte(id)
		c_addr = c_ubyte(address)
		c_size = c_ubyte(size)
		r = self.lib.GetParameters(cid, c_addr, c_size)
		return r    

   #扫描在线机器人设备
	def ScanRobots(self):
	    charPP = POINTER(c_char_p)
	    ipPPvalue = c_char_p(5)
	    ipPP = charPP(ipPPvalue)
	    int_p = POINTER(c_int)
	    countObject = c_int(4)
	    count = int_p(countObject)
	    num = 0
	    num = self.lib.ScanRobots(ipPP,count)
	    robotsLs = []
	    for i in range(num):
	      robotsLs.append(str(ipPP[i]))
	    return robotsLs


   #扫描在线机器人设备:58899 仿真器
	def ScanRobotsWithPort(self,port):
	    charPP = POINTER(c_char_p)
	    ipPPvalue = c_char_p(5)
	    ipPP = charPP(ipPPvalue)
	    int_p = POINTER(c_int)
	    countObject = c_int(4)
	    count = int_p(countObject)
	    num = 0
	    num = self.lib.ScanRobotsWithPort(c_int(port),ipPP,count)
	    robotsLs = []
	    for i in range(num):
	      robotsLs.append(b2Str(ipPP[i]))
	    return robotsLs

	#扫描在线机器人名称获取IP地址以连接机器人
	def ScanRobotWithName(self, robotName, port=8899):
		rbs= self.ScanRobots()
		ipAddr = None
		if len(rbs) > 0:
				robot_num = len(rbs)
				for robot_info in rbs:
					tmp = robot_info.split(',')
					ip_addr = tmp[0]
					device_name =  tmp[2]
					if robotName in device_name:
						ipAddr = ip_addr
						COUNT += 1
		res = self.Connect(ipAddr,port)
		if (res>0):
			print("连接成功")
		else:
			print("连接失败")
			exit()

    #连接目标机器人设备
	def Connect(self,ip, port):
	     r = self.lib.Connect(c_char_p(ip.encode('utf-8')),c_int(port))
	     return r
	   
    #连接机器人（带超时设置）
	def ConnectWithTimeOut(self,ip, port,nNetTimeout):
	     r = self.lib.ConnectWithTimeOut(c_char_p(ip.encode('utf-8')),c_int(port),c_int(nNetTimeout))
	     return r

   #断开目标机器人设备
	def Disconnect(self):
		r = self.lib.Disconnect()
		return r


   #扫描机器人底座连接的所有模块ID【返回Id 及 对应位置 列表】
	def ScanId(self,startId, endId):
		self.lib.ScanId.argtypes = [c_ubyte,c_ubyte,POINTER(c_ubyte),POINTER(c_ubyte)]
		self.lib.ScanId.restype = c_int
		id = c_ubyte(0)
		position = c_ubyte(0)
		num = self.lib.ScanId(startId,endId,id,position)
		id_p = pointer(id)
		position_p = pointer(position)

		# print("Total Ids Count=" + str(num))
		idLs = []
		posLs = []
		for i in range(num):
			# print("Id=" + str(id_p[i]))
			idLs.append(id_p[i])
		for i in range(num):
		 # print("Position=" + str(position_p[i]))
		 posLs.append(position_p[i])
		return idLs,posLs


   #获取指定ID关节模块的位置角度值，非堵塞
	def GetPosition(self,id):
		cid = c_ubyte(id)
		self.lib.GetPosition.restype = c_int
		angle = self.lib.GetPosition(cid)
		return angle

   #获取指定ID关节模块的速度信息-【关节模块处于速度模式】
	def GetRunningSpeed(self,id):
		cid = c_ubyte(id)
		self.lib.GetPosition.restype = c_int
		speed = self.lib.GetRunningSpeed(cid)
		return speed

   #获取指定ID关节模块的速度信息-【关节处于位置模式】
	def GetPositionSpeed(self,id):
		cid = c_ubyte(id)
		self.lib.GetPosition.restype = c_int
		speed = self.lib.GetPositionSpeed(cid)
		return speed

	# 根据模块关节运行速度，计算出需要sleep的时间
	def ComputeDelayTime(self, diff, speed):
		ang_to_step = diff * 4096.0 / 360
		delay_time = ang_to_step / (3400.0 * speed / 100)
		return delay_time+0.15

   #设置关节模块位置角度（位置模式）
	def SetPosition(self,id,angle): 
		r = self.lib.SetPosition(c_ubyte(id),c_double(angle)) 
		return r
#    #设置关节模块位置角度（位置模式）
# 	def SetPosition(self,id,angle): 
# 		speed = 30
# 		self.lib.SetMotorMode(id,0)  
# 		self.lib.SetPositionSpeed(id, speed)
# 		cur_ang = self.lib.GetPosition(id)
# 		diff = abs(angle -  cur_ang)
# 		t =self.ComputeDelayTime(diff,speed)
# 		r = self.lib.SetPosition(c_ubyte(id),c_double(angle)) 
# 		time.sleep(t)
# 		return r

	#设置关节模块位置角度（位置模式）
	def SetIdsPositions(self,ids,angles): 
		size = len(ids)
		for i in range(size):
			self.SetPosition(ids[i],angles[i])

   #设置关节模块以指定速度运行到指定位置角度
	def SetPositionWithSpeed(self,id,angle,speed):
		r = self.lib.SetPositionWithSpeed(c_ubyte(id),c_double(angle),c_int(speed))
		time.sleep(0.3)
		return r

   	#设置多个关节模块以指定速度运行到指定位置角度
	def SetIdsPositionsWithSpeed(self,ids,angles,speeds): 
		size = len(ids)
		for i in range(size):
			self.SetMotorMode(ids[i], 0)
			self.SetPositionWithSpeed(ids[i],angles[i],speeds[i])
			

   # 设置指定ID关节模块的运行速度，会自动将关节模块设置为速度模式。该设置仅对I模块有效
	def SetRunningSpeed(self,id,speed):
		r = self.lib.SetRunningSpeed(c_ubyte(id),c_int(speed))
		return r

   #  设置关节模块运行到指定角度的速度
	def SetPositionSpeed(self,id,speed):
		r = self.lib.SetPositionSpeed(c_ubyte(id),c_int(speed))
		return r


   # 设置指定ID关节是否使能：id,isEnable(0：关；1：开启使能)
	def EnableTorque(self,id,isEnable):
		r = self.lib.EnableTorque(c_ubyte(id),bool(isEnable))
		return r


   #设置关节模块模式：mode(0位置,1速度,2开环调速度模式)
	def SetMotorMode(self,id,mode):
		r = self.lib.SetMotorMode(c_ubyte(id),c_int(mode))
		return r

   # 同步设置多个关节模块的位置
	def SetPositionSync(self,ids,angles):
		size = len(ids)
		idLs = (c_ubyte * size)()
		posLs = (c_double * size)()
		for i in range(size):
			idLs[i] = (c_ubyte(ids[i]))
			posLs[i] = (c_double(angles[i]))    
		r = self.lib.SetPositionSync(idLs,posLs,c_int(size))
		return r

 	# 以一定的速度同步设置多个关节模块的位置
	def SetPositionSyncWithSpeeds(self,ids,angles,speeds):
		size = len(ids)
		idLs = (c_ubyte * size)()
		posLs = (c_double * size)()
		speedLs = (c_int * size)()
		
		for i in range(size):
			idLs[i] = (c_ubyte(ids[i]))
			posLs[i] = (c_double(angles[i]))    
			speedLs[i] = (c_int(speeds[i]))
		r = self.lib.SetPositionSyncWithSpeeds(idLs,posLs,speedLs,c_int(size))
		return r

	 # 同步设置多个关节模块的速度
	def SetSpeedSync(self,ids,speeds):
		size = len(ids)
		idLs = (c_ubyte * size)()
		speedLs = (c_int * size)()
		
		for i in range(size):
			idLs[i] = (c_ubyte(ids[i]))  
			speedLs[i] = (c_int(speeds[i]))
		r = self.lib.SetSpeedSync(idLs,speedLs,c_int(size))
		return r
   #设置模块的ID，设置的ID应满足以下ID段，否则使用时可能会造成硬件损伤
	def SetMotoId(self,oldId,newId):
		r = self.lib.SetMotoId(c_ubyte(oldId),c_ubyte(newId))
		return r

   #设置模块是否锁定不可写id,isLock(0：锁关闭；1：锁开启)
	def SetMotorLocked(self,id,isLock):
		r = self.lib.SetMotorLocked(c_ubyte(id),bool(isLock))
		return r


   #设置当前连接的机器人设备的连接模式为STA模式，即连接到公网WIFI热点 注：不支持5G网络
	def SetSTAMode(self,ssid,pwd):
		r = self.lib.SetSTAMode(bytes(ssid),bytes(pwd))
		return r

   #设置当前连接的机器人设备的连接模式为AP模式，即将当前设备设置为可连接的热点
	def SetAPMode(self,ssid,pwd):
		r = self.lib.SetAPMode(bytes(ssid),bytes(pwd))
		return r

   #设置小移动平台灯带模式（如果存在）
	def SetLEDBel(self,val):
		self.lib.SetLEDBel.restype = c_int
		r = self.lib.SetLEDBel(c_int(val))
		return r

   #读取外部设备数据(需连接传感器模块)
   # id：设备ID  ioPort：控制板接口   
	def GetDevData(self,id,ioPort):
		r = self.lib.GetDevData(c_ubyte(id),c_ubyte(ioPort))
		return r

   #设置传感器数据
   # id：设备ID
   # ioPort：控制板接口
   # data：期望的数据
   # data的长度由字节数确定，例如4位数码管字节数为4，则data=[0,0,0,0]，data里面的数字既可以10进制表示，也可以16进制表示，大小由字节范围确定
   # 例如data=[0,0,0,0x16]是不可以的，因为超出了字节范围
	def SetDevData(self,id,ioPort,data):
		if type(data) == list:
			dataLen = len(data)
			data_array = (c_ubyte * dataLen)()
			for i in range(dataLen):
				data_array[i] = (c_ubyte(data[i]))
		elif type(data) == int:
			dataLen = 1
			data_array = (c_ubyte * dataLen)()
			data_array[0] = (c_ubyte(data))

		r = self.lib.SetDevData(c_ubyte(id),c_ubyte(ioPort),data_array,c_int(dataLen))
		return r

   #从数据缓冲区中读取设备数据（例如：红外按钮）
	def ReadDevData(self,id,ioPort):
		r = self.lib.ReadDevData(c_ubyte(id),c_ubyte(ioPort))
		return r

   #设置模块高压保护值
	def SetMotoHighVoltageProtection(self,id,value):
		r = self.lib.SetMotoHighVoltageProtection(c_ubyte(id),c_ubyte(value))
		return r

   #设置模块低压保护值
	def SetMotoLowVoltageProtection(self,id,value):
		r = self.lib.SetMotoLowVoltageProtection(c_ubyte(id),c_ubyte(value))
		return r

   #设置模块的最大角度限制
	def SetMotoMaxAngle(self,id,angle):
		r = self.lib.SetMotoMaxAngle(c_ubyte(id),c_int(angle))
		return r

   #设置模块的最小角度限制
	def SetMotoMinAngle(self,id,angle):
		r = self.lib.SetMotoMinAngle(c_ubyte(id),c_int(angle))
		return r

   #设置PWM(运行时间)
	def SetPWM(self,id,value):
		r = self.lib.SetPWM(c_ubyte(id),c_int(value))
		return r

   #获取PWM(运行时间)
	def GetPWM(self,id):
		int_Point = POINTER(c_int)
		pwm = c_int(0)
		pwm_p = int_Point(pwm)
		r = self.lib.GetPWM(c_ubyte(id),pwm_p)
		return pwm_p.contents

   #设置位置环P 比例系数
	def SetRingP(self,id,value):
		r = self.lib.SetPWM(c_ubyte(id),c_ubyte(value))
		return r

   #设置位置环D 微分系数
	def SetRingD(self,id,value):
		r = self.lib.SetPWM(c_ubyte(id),c_ubyte(value))
		return r

   #设置位置环I 积分系数
	def SetRingI(self,id,value):
		r = self.lib.SetPWM(c_ubyte(id),c_ubyte(value))
		return r

   #设置卸载条件
	def SetProtectConditions(self,id,isVoltage,isSensor,isTemperature,isElectricity,isAngle,isOverload):
		r = self.lib.SetProtectConditions(c_ubyte(id),bool(isVoltage),bool(isSensor),bool(isTemperature),bool(isElectricity),bool(isAngle),bool(isOverload))
		return r


   #设置LED报警条件
	def SetLedConditions(self,id,isVoltage,isSensor,isTemperature,isElectricity,isAngle,isOverload):
		r = self.lib.SetLedConditions(c_ubyte(id),bool(isVoltage),bool(isSensor),bool(isTemperature),bool(isElectricity),bool(isAngle),bool(isOverload))
		return r
